function simplifiedPath = rdp(points, epsilon)
    % 找到离首尾连线最远的点
    dmax = 0;
    idx = 1;
    n = size(points, 1);
    
    if n <= 2
        simplifiedPath = points;
        return;
    end
    
    % 计算所有点到首尾连线的距离
    p1 = points(1, :);
    p2 = points(end, :);
    d = zeros(n, 1);
    
    for i = 2:n-1
        d(i) = point_to_line_distance(points(i, :), p1, p2);
        if d(i) > dmax
            dmax = d(i);
            idx = i;
        end
    end
    
    % 递归简化
    if dmax > epsilon
        left = rdp(points(1:idx, :), epsilon);
        right = rdp(points(idx:end, :), epsilon);
        simplifiedPath = [left(1:end-1, :); right];
    else
        simplifiedPath = [p1; p2];
    end
end

function d = point_to_line_distance(pt, p1, p2)
    % 计算点 pt 到 p1-p2 连线的垂直距离
    if all(p1 == p2)
        d = norm(pt - p1);
    else
        d = abs((p2(1)-p1(1))*(p1(2)-pt(2)) - (p1(1)-pt(1))*(p2(2)-p1(2))) / norm(p2 - p1);
    end
end